#include "control.h"


//全工程维护，方向环PID更新此值
volatile int servo_duty;
// 全工程维护,左后轮占空比，用在set_motor_speed函数里
volatile int duty_left;
// 全工程维护,右后轮占空比，用在set_motor_speed函数里
volatile int duty_right;

// 全工程维护，中线斜率
float slope_k;
// 全工程维护，预瞄误差
volatile float preaim_error;

/**
 * @brief   计算赛道中线的斜率 (优化版本)
 * @details 此函数通过最小二乘法拟合赛道中线的离散点来计算其整体斜率。
 * @note    此函数会更新全局变量 slope_k。
 *          采样的点集为 center_line_points 数组中从索引110到10，步长为5的有效点。
 */
void calculate_slope(void)
{
    // --- 最小二乘法累加变量 ---
    // 使用 double 类型以保证计算过程中的精度
    double sum_x = 0.0;
    double sum_y = 0.0;
    double sum_xy = 0.0;
    double sum_x2 = 0.0;
    int n_k = 0; // 有效数据点的数量

    // --- 单次遍历直接计算累加和 ---
    // 从图像较远处(行号110)向近处(行号10)采样，步长为5
    for (int i = 110; i > 5; i -= 5)
    {
        // 检查中线点是否有效 (-1 为无效标记)
        if (center_line_points[i] != -1)
        {
            // 累加计算最小二乘法所需各项
            // x 坐标是图像的行号 i
            // y 坐标是该行的中线列坐标 center_line_points[i]
            sum_x += i;
            sum_y += center_line_points[i];
            sum_xy += (double)i * center_line_points[i];
            sum_x2 += (double)i * i;
            n_k++; // 有效点计数增加
        }
    }

    // --- 计算斜率 ---
    // 必须保证有至少2个点才能拟合一条直线
    if (n_k >= 2)
    {
        // 最小二乘法斜率 a 的计算公式:
        // a = (n * Σxy - Σx * Σy) / (n * Σx² - (Σx)²)
        double denominator = (double)n_k * sum_x2 - sum_x * sum_x;

        // 严谨性检查：防止除以零
        if (denominator != 0.0)
        {
            slope_k = ((double)n_k * sum_xy - sum_x * sum_y) / denominator;
        }
        else
        {
            // 如果所有采样点的x坐标都相同（在此场景下不可能发生），
            // 或者出现其他浮点数精度问题，斜率未定义。
            // 将斜率置为0作为一个安全的回退值。
            slope_k = 0.0f;
        }
    }
    else
    {
        // 如果有效点不足2个，无法计算斜率，同样置为0。
        slope_k = 0.0f;
    }
}

// --- 配置参数 ---
#define PREAIM_ROW_START 60 // 预瞄计算的起始行
#define PREAIM_ROW_COUNT 5  // 预瞄计算的采样行数
/**
 * @brief   计算预瞄误差。
 * @details 通过计算预瞄区域内赛道中线的平均位置，来确定车辆的横向误差。
 *          此函数是可重入的，除了修改核心输出 `preaim_error` 外无其他副作用。
 *
 * @note    读取全局变量: `center_line_points[]` (赛道中线数据)
 *          写入全局变量: `preaim_error` (计算出的预瞄误差)
 *          依赖宏: `PREAIM_ROW_START`, `PREAIM_ROW_COUNT`
 */
float calculate_preaim_error(void)
{
    // 定义局部变量进行累加计算，避免使用全局变量
    float feedback_sum = 0.0f;
    int valid_points_count = 0;
    float preaim_error_local = 0.0f;
    // 遍历预瞄区域，累加有效的中线点坐标
    for (int i = PREAIM_ROW_START; i < (PREAIM_ROW_START + PREAIM_ROW_COUNT); i++)
    {
        // 检查中线点是否有效 (-1 为无效标记)
        if (center_line_points[i] != -1)
        {
            feedback_sum += center_line_points[i];
            valid_points_count++;
        }
    }
    // 仅在找到至少一个有效点时才更新误差，防止除以零
    if (valid_points_count > 0)
    {
        // 计算平均中点位置
        const float average_center = feedback_sum / valid_points_count;

        // 计算图像理论中心位置 (使用 2.0f 保证浮点数除法)
        const float image_center = (MT9V03X_W - 1) / 2.0f;
        // 更新核心输出：预瞄误差 = 理论中心 - 实际中心
        preaim_error = image_center - average_center;
    }
    else
    {
        // 异常处理：若预瞄区无有效点，则不更新误差，保持上一帧的有效值。
        // 这可以防止控制系统因单帧的坏数据而产生剧烈抖动。
    }

    return preaim_error;
}

/**
 * @brief 计算赛道宽度
 *
 */
void calculate_track_width()
{
    for (int row = 0; row < 120; row++)
    {
        if (left_edge_points[row] != -1 && right_edge_points[row] != -1)
        {
            track_width[row] = right_edge_points[row] - left_edge_points[row];
        }
        else
        {
            track_width[row] = -1;
        }
    }
    // 存入flash
    uint32 buff_data[120];
    memcpy(buff_data, track_width, 480);
    flash_erase_page(127, FLASH_PAGE_1);
    flash_write_page(127, FLASH_PAGE_1, buff_data, 120);
}

/**
 * @brief 计算赛道预瞄点误差
 * @note 方案待定，因为涉及到硬件flash操作，故不做优化
 */
void calculate_track_error()
{
    // 存入flash
    uint32 buff_data[1];
    memcpy(buff_data, &preaim_error, 4);
    flash_erase_page(125, FLASH_PAGE_1);
    flash_write_page(125, FLASH_PAGE_1, buff_data, 1);
    //
}



static  float dif_outer_history_data[3]={0.0f};//外轮历史数据记录
static  float dif_inner_history_data[3]={0.0f};//内轮历史数据记录
static const float dif = 3.2f; //占空比到打角的比例系数
static const float wheel_width = 15.5f; //轮距
static const float wheelbase = 21; //轴距
static const float w_b_ratio = wheel_width/(2*wheelbase);
static const float A_T_R= 0.01745329252f;//角度转弧度

/**
 * @brief 阿克曼控速
 * 
 * @param Speed_Real 期望真实速度(m/s) 
 * @param EDS_Coefficient_Out 输出系数
 * @param EDS_Coefficient_In  输入系数
 */
void dir_speed_control(float Speed_Real, int servo_offset, float EDS_Coefficient_Out, float EDS_Coefficient_In)
{   
    float expect_left_speed = 0;
    float expect_right_speed=0;
    //死区内，打角过小，直接直行，并清空历史记录
    if(fabs(servo_offset) < 30.0f)
    {

        for(int i=0;i<3;i++)
        {
            dif_outer_history_data[i]=0.0f;
            dif_inner_history_data[i]=0.0f;
        }

        return;
    }

    float angle_t =servo_offset / dif;

    //限幅
    if(angle_t<=-9.1f) {angle_t=-9.1f;}
    if(angle_t>=9.1f) {angle_t=9.1f;}
    //0是当前,1是上次,2是上上次
    //外轮控制
    dif_outer_history_data[0]=w_b_ratio*(1+tan(EDS_Coefficient_Out*angle_t*A_T_R))*Speed_Real;

    dif_outer_history_data[2]=dif_outer_history_data[1];
    dif_outer_history_data[1]=dif_outer_history_data[0];
    //历史数据融合，滑动平滑滤波器

    //外轮期望速度
    float expect_outer_speed=dif_outer_history_data[0]*0.4f+dif_outer_history_data[1]*0.4f+dif_outer_history_data[2]*0.2f;

     //内轮控制                                                                          
    dif_inner_history_data[0] = w_b_ratio*(1-tan(EDS_Coefficient_In * angle_t * A_T_R)) * Speed_Real;

    dif_inner_history_data[2]=dif_inner_history_data[1];
    dif_inner_history_data[1]=dif_inner_history_data[0];

    //内轮期望速度
    float expect_inner_speed=dif_inner_history_data[0]*0.4f+dif_inner_history_data[1]*0.4f+dif_inner_history_data[2]*0.2f;


    if(servo_duty>0)//左转
    {   
        //左轮内轮减速
        //与历史数据做融合，滑动平均滤波器
        duty_left=pid_calculate(&pid_speed,expect_inner_speed,left_encoder_count);
        //右轮外轮加速
        duty_right=pid_calculate(&pid_speed,expect_outer_speed,right_encoder_count);

    }

    else//右转
    {   
        //左轮外轮加速
        duty_left=pid_calculate(&pid_speed,expect_outer_speed,left_encoder_count);
        // 右轮内轮减速
        duty_right=pid_calculate(&pid_speed,expect_inner_speed,right_encoder_count);
       
    }


}
